clc;clear;close;

% r_arm
% theta = [pi/2, -pi/2-0.4677, pi, pi, pi/2];
a=[0, 0, -0.01, 0, 0];
theta = [0/180*pi, 0/180*pi, 0/180*pi, 0/180*pi, 0/180*pi];
d = [0.1096, 0, 0.2765, 0, 0.1265];

T_01 = getT_ij(pi/2, a(1), theta(1), 109.6);
euler = rotm2eul(T_01(1:3, 1:3));

T_12 = getT_ij(pi/2, a(2), theta(2), 0);
euler = rotm2eul(T_12(1:3, 1:3));

T_02 = T_01*T_12;
euler = rotm2eul(T_02(1:3, 1:3));

T_23 = getT_ij(pi/2, a(3), theta(3), 276.5);
euler = rotm2eul(T_23(1:3, 1:3));

T_03 = T_02*T_23;
euler = rotm2eul(T_03(1:3, 1:3));

T_34 = getT_ij(-pi/2, a(4), theta(4), 0);
euler = rotm2eul(T_23(1:3, 1:3));

T_45 = getT_ij(pi/2, a(5), theta(5), 126.5);
euler = rotm2eul(T_23(1:3, 1:3));

T_05 = T_01*T_12*T_23*T_34*T_45;
euler = rotm2eul(T_05(1:3, 1:3));

% r_leg
% T_01 = getT_ij(0.2618, 0, pi/2, 106.5);
% euler = rotm2eul(T_01(1:3, 1:3));
% 
% T_12 = getT_ij(-pi/2, 0, -pi/2, 14.65);
% euler = rotm2eul(T_12(1:3, 1:3));
% 
% T_02 = T_01*T_12;
% euler = rotm2eul(T_02(1:3, 1:3));
% 
% T_23 = getT_ij(pi/2, -0.12, 0, 387.85);
% euler = rotm2eul(T_23(1:3, 1:3));
% 
% T_03 = T_02*T_23;
% euler = rotm2eul(T_03(1:3, 1:3));
% 
% T_34 = getT_ij(-pi/2, 0, 0, 30.50);
% euler = rotm2eul(T_34(1:3, 1:3));

% T_04 = T_01*T_12*T_23*T_34;
% euler = rotm2eul(T_04(1:3, 1:3));

function T_ij = getT_ij(alpha, a, theta, d)
    T_ij=[
        [cos(theta), -sin(theta), 0, a];
        [sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha)];
        [sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), d*cos(alpha)];
        [0, 0, 0, 1]];
end



